#include "gtrack_hough.h"
#include "track_unitConfidence.h"
#include <stdio.h>
#include <stdlib.h>

bool houghDetection(void* data_x, void* data_y)
{
    MotionWindow* pointsX  = (MotionWindow*)data_x;
    MotionWindow* pointsY = (MotionWindow*)data_y;
    bool b_line = false;
    // 1.初始化霍夫累加器
    int theta_bins = (int)((THETA_MAX - THETA_MIN) / D_THETA);
    int rho_bins = (int)(2 * RHO_MAX / D_RHO);
    int** accumulator = (int**)malloc(rho_bins * sizeof(int*));
    for (int i = 0; i < rho_bins; i++)
    {
        accumulator[i] = (int*)calloc(theta_bins, sizeof(int));
    }
    int num_points = pointsX->valid_count < pointsY->valid_count ? pointsX->valid_count : pointsY->valid_count;
    if (num_points < TRACK_WINDOW_SIZE)
    {
        return false;
    }
    // 3. 执行霍夫投票
    for (int p = 0; p < num_points; p++)
    {
        for (float theta = THETA_MIN; theta < THETA_MAX; theta += D_THETA)
        {
            float rho = pointsX->buffer[p] * cosf(theta) + pointsY->buffer[p] * sinf(theta);
            int rho_idx = (int)((rho + RHO_MAX) / D_RHO);
            int theta_idx = (int)((theta - THETA_MIN) / D_THETA);
            if (rho_idx >= 0 && rho_idx < rho_bins && theta_idx >= 0 && theta_idx < theta_bins)
            {
                accumulator[rho_idx][theta_idx]++;
            }
        }
    }

    // 4. 检测峰值（候选轨迹）
    for (int r = 0; r < rho_bins; r++)
    {
        for (int t = 0; t < theta_bins; t++)
        {
            if (accumulator[r][t] >= VOTE_THRESHOLD)
            {
                float detected_rho = (r * D_RHO) - RHO_MAX;
                float detected_theta = THETA_MIN + t * D_THETA;

                // 验证所有点是否在检测到的直线上
                int valid_count = 0;
                for (int i = 0; i < num_points; i++)
                {
                    float calc_rho = pointsX->buffer[i] * cosf(detected_theta) + pointsY->buffer[i] * sinf(detected_theta);
                    if (fabsf(calc_rho - detected_rho) < TOLERANCE)
                        valid_count++;
                }

                if (valid_count >= VOTE_THRESHOLD)
                {
                    b_line = true;
                    //printf("检测到直线: rho=%.2f, theta=%.2f度, 支持点数=%d/%d\n", detected_rho, detected_theta * 180 / M_PI, valid_count, num_points);
                }
            }
        }
    }

    // 清理内存
    for (int i = 0; i < rho_bins; i++) free(accumulator[i]);
    free(accumulator);

    return b_line;
}